% clear;
% clc;
% close all;

%% 参数设置
fs = 96e6;            % 采样频率 96MHz
fs_out = 4e6;         % 输出采样频率 4MHz
D = fs/fs_out;        % 降采样因子 = 24
N = 5;                % CIC滤波器阶数
M = 1; % 梳状器差分延迟（固定为1，简化实现）
f_sig = 400e3;           % 输入信号频率 400kHz
snr = 20;             % 信噪比
input_bits = 4;           % 输入位宽
max_input_value = 2^input_bits - 1;  % Maximum input value (15 for 4-bit)
simulation_time = 1e-3; % 仿真时间 1ms

%% 生成SDADC输入信号
% x(n)=x(nTs)
t = 0:1/fs:(simulation_time-1/fs);  %通过给定的采样频率来控制每个信号样本的时间间隔，以这个频率进行时间离散化
s = sin(2*pi*f_sig*t);  % 生成400kHz正弦波, 96MHz采样后的离散信号s
x = awgn(s, snr);    % 添加高斯白噪声

%% SDADC模型
num = [1, -2.994219815030523, 2.994219815030523, -1];
den = [1, -2.33063271405379, 1.866676648819709, -0.508718943549610];
y = filter(num, den, x);  % SDM输出
% num和den定义的IIR滤波器是噪声整形滤波器（Noise-Shaping Filter）

% 定义滤波器系数
num = [1, -2.994219815030523, 2.994219815030523, -1];
den = [1, -2.33063271405379, 1.866676648819709, -0.508718943549610];

% % 计算频率响应
% [h, w] = freqz(num, den, 1024); % 计算1024个点的频率响应
% 
% % 绘制幅频响应(幅度)
% figure;
% subplot(2,1,1);
% plot(w/pi, 20*log10(abs(h)));
% grid on;
% title('幅频响应 (dB)');
% xlabel('归一化频率 (\times\pi rad/sample)');
% ylabel('幅度 (dB)');
% 
% % 绘制相频响应(相位)
% subplot(2,1,2);
% plot(w/pi, angle(h));
% grid on;
% title('相频响应');
% xlabel('归一化频率 (\times\pi rad/sample)');
% ylabel('相位 (radians)');


%% 将SDADC输出量化为4位无符号数（0-15）
% 更鲁棒的归一化方法（防止除零）
y_range = max(y) - min(y);
if y_range == 0
    y_range = 1; % 处理直流信号情况
end
y_normalized = (y - min(y)) / y_range;
% 量化为4位，即0-15的整数
y_quantized = round(y_normalized * max_input_value);
% 确保量化后的结果在0-15范围内
y_quantized = max(0, min(max_input_value, y_quantized));

%% 生成Verilog测试文件
fid = fopen('cic_input.txt', 'w');
fprintf(fid, '%x\n', y_quantized); % 16进制格式写入
fclose(fid);

% %% 频谱分析
% % 绘制SDADC输出信号频谱
% N = length(y);          % 信号长度
% Y = fft(y, N);          % 计算FFT
% Y_abs = abs(Y/N);       % 计算幅度谱并归一化
% Y_abs = Y_abs(1:N/2+1); % 只取一半频谱（由于对称性）
% Y_abs(2:end-1) = 2*Y_abs(2:end-1); % 校正单边频谱
% 
% % 计算频率轴
% f = fs * (0:(N/2))/N;
% 
% % 绘制频谱
% figure;
% plot(f/1e6, 20*log10(Y_abs)); % 转换为dB并绘制
% title('SDADC输出信号频谱');
% xlabel('频率 (MHz)');
% ylabel('幅度 (dB)');
% grid on;
% % % xlim([0 1]);  % 将频谱缩放到感兴趣的频率范围
% 
% % 绘制量化后信号的频谱
% Y_q = fft(y_quantized, N);
% Y_q_abs = abs(Y_q/N);
% Y_q_abs = Y_q_abs(1:N/2+1);
% Y_q_abs(2:end-1) = 2*Y_q_abs(2:end-1);
% 
% figure;
% plot(f/1e6, 20*log10(Y_q_abs));
% title('量化后SDADC输出信号频谱');
% xlabel('频率 (MHz)');
% ylabel('幅度 (dB)');
% grid on;
% % % xlim([0 1]);

%% CIC滤波器设计
% 计算位宽
bit_growth = N * log2(D*M);
output_bits = input_bits + ceil(bit_growth); % 计算输出位宽
mod_value = 2^output_bits;    % 模值，用于处理溢出

% 初始化积分器状态
integrator_states = zeros(1, N);

% 初始化梳状器延迟（对于M=1，每个梳状器只需一个延迟单元）
comb_delays = zeros(1, N);  % 所有梳状器延迟单元初始值为0
% 初始化输出数组
output_length = ceil(length(y_quantized)/D);  % 降采样后的输出长度
cic_output = zeros(1, output_length);         % 预分配输出数组

% 处理每个输入样本
output_idx = 1;  % 输出索引
for n = 1:length(y_quantized)
    % 第一阶段：级联积分器处理
    x_tmp = y_quantized(n);  % 获取当前输入样本
    
    % 通过N个级联积分器（每个都是一个累加器）
    for i = 1:N
        % 积分器方程: y[n] = y[n-1] + x[n]
        integrator_states(i) = mod(integrator_states(i) + x_tmp, mod_value);
        x_tmp = integrator_states(i);  % 当前积分器的输出作为下一个积分器的输入
    end
    
    % 第二阶段：抽取（降采样）
    % 只在每D个样本点处理一次梳状器级联（实现降采样）
    if mod(n-1, D) == 0
        % 第三阶段：级联梳状器处理
        % 在对信号进行抽取后，通过N个级联梳状器
        for i = 1:N
            % 梳状器方程: y[n] = x[n] - x[n-M]
            % 对于M=1，就是当前输入减去上一样本值
            y_tmp = mod(x_tmp - comb_delays(i), mod_value);  % 使用模运算处理下溢
            
            % 更新梳状器延迟值（保存当前输入作为下次使用）
            comb_delays(i) = x_tmp;
            
            x_tmp = y_tmp;  % 当前梳状器的输出作为下一个梳状器的输入
        end
        
        % 保存最终输出
        cic_output(output_idx) = x_tmp;
        output_idx = output_idx + 1;
    end
end

%% 将无符号输出转换为有符号（二进制补码表示还原为有符号数值）
cic_output_signed = zeros(size(cic_output));
for i = 1:length(cic_output)
    if cic_output(i) >= mod_value/2
        % 如果最高位为1，则为负数（二进制补码表示）
        cic_output_signed(i) = cic_output(i) - mod_value;
    else
        % 如果最高位为0，则为正数
        cic_output_signed(i) = cic_output(i);
    end
end


%% 保存输出为文件 (用于Verilog验证)
% 保存为文本文件
fid = fopen('cic_output.txt', 'w');
for i = 1:length(cic_output)
    fprintf(fid, '%x\n', cic_output(i));
end
fclose(fid);

%% 使用 dsp.CICDecimator 实现参考滤波器
cic_ref = dsp.CICDecimator('DecimationFactor', D, ...
                           'DifferentialDelay', M, ...
                           'NumSections', N);
output_ref = cic_ref(y_quantized(:)); 
output_ref = output_ref';

%% 将无符号输出转换为有符号（二进制补码表示还原为有符号数值）
output_ref_signed = zeros(size(output_ref));
for i = 1:length(output_ref)
    if output_ref(i) >= mod_value/2
        % 如果最高位为1，则为负数（二进制补码表示）
        output_ref_signed(i) = output_ref(i) - mod_value;
    else
        % 如果最高位为0，则为正数
        output_ref_signed(i) = output_ref(i);
    end
end


t_temp=0:1/fs_out:3999/fs_out;

plot(t_temp, cic_output_signed/(1e7), ...
    'Color', [0, 0.4470, 0.7410], ...
    'LineWidth', 1.5, ...
    'DisplayName', 'CIC Output'); 
hold on;
plot(t_temp, output_ref_signed/(1e7), ...
    'Color', [0.8500, 0.3250, 0.0980], ...
    'LineWidth', 1.5, ...
    'DisplayName', 'Reference Output');
hold off;
